
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_airspeed_ms5525.c
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed_ms5525.h"

#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
#include <analogin_manager/analogin.h>
#include <common/utility/sparse-endian.h>
/*-----------------------------------macro------------------------------------*/
#define MS5525D0_I2C_ADDR_1 0x76
#define MS5525D0_I2C_ADDR_2 0x77

#define REG_RESET               0x1E
#define REG_CONVERT_D1_OSR_256  0x40
#define REG_CONVERT_D1_OSR_512  0x42
#define REG_CONVERT_D1_OSR_1024 0x44
#define REG_CONVERT_D1_OSR_2048 0x46
#define REG_CONVERT_D1_OSR_4096 0x48
#define REG_CONVERT_D2_OSR_256  0x50
#define REG_CONVERT_D2_OSR_512  0x52
#define REG_CONVERT_D2_OSR_1024 0x54
#define REG_CONVERT_D2_OSR_2048 0x56
#define REG_CONVERT_D2_OSR_4096 0x58
#define REG_ADC_READ            0x00
#define REG_PROM_BASE           0xA0

// go for 1024 oversampling. This should be fast enough to reduce
// noise but low enough to keep self-heating small
#define REG_CONVERT_PRESSURE    REG_CONVERT_D1_OSR_1024
#define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool init(sensor_as_backend_t as_backend);
static bool get_differential_pressure(sensor_as_backend_t as_backend, float *_pressure);
static bool get_temperature(sensor_as_backend_t as_backend, float *_temperature);
static uint16_t crc4_prom(sensor_as_ms5525_t ms5525);
static bool read_prom(sensor_as_ms5525_t ms5525);
static int32_t read_adc(sensor_as_ms5525_t ms5525);
static void calculate(sensor_as_ms5525_t ms5525);
static void timer(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_as_backend_ops ms5525_ops = {.as_backend_destructor = NULL,
                                                  .init = init,
                                                  .get_differential_pressure = get_differential_pressure,
                                                  .get_temperature = get_temperature,
                                                  .has_airspeed = NULL,
                                                  .get_airspeed = NULL,
#if MB_AIRSPEED_HYGROMETER_ENABLE
                                                  .get_hygrometer = NULL
#endif
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_as_ms5525_ctor(sensor_as_ms5525_t ms5525, uint8_t _instance, enum MS5525_ADDR address)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(ms5525, 0, sizeof(struct sensor_as_ms5525));

    sensor_as_backend_ctor(&ms5525->backend, &ms5525_ops, "ms5525", _instance);

    ms5525->_address = address;
}

sensor_as_backend_t sensor_as_ms5525_probe(uint8_t _instance, enum MS5525_ADDR address)
{
    sensor_as_ms5525_t sensor = (sensor_as_ms5525_t)rt_malloc(sizeof(struct sensor_as_ms5525));

    if (!sensor) {
        return NULL;
    }

    sensor_as_ms5525_ctor(sensor, _instance, address);

    return (sensor_as_backend_t)sensor;
}

// probe and initialise the sensor
static bool init(sensor_as_backend_t as_backend)
{
    sensor_as_ms5525_t ms5525 = (sensor_as_ms5525_t)as_backend;

    const uint8_t addresses[] = { MS5525D0_I2C_ADDR_1, MS5525D0_I2C_ADDR_2 };
    bool found = false;
    for (uint8_t i=0; i<ARRAY_SIZE(addresses); i++) {
        if (ms5525->_address != MS5525_ADDR_AUTO && i != (uint8_t)ms5525->_address) {
            continue;
        }
        ms5525->dev = devmgr_get_i2c_device(sensor_as_backend_get_bus(as_backend), addresses[i]);
        if (!ms5525->dev) {
            continue;
        }

        // lots of retries during probe
        devmgr_set_retries(ms5525->dev, 5);

        found = read_prom(ms5525);
        
        if (found) {
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS5525[%u]: Found on bus %u addr 0x%02x", sensor_as_backend_get_instance(as_backend), sensor_as_backend_get_bus(as_backend), addresses[i]);
            console_printf("MS5525[%u]: Found on bus %u addr 0x%02x", sensor_as_backend_get_instance(as_backend), sensor_as_backend_get_bus(as_backend), addresses[i]);
            break;
        }
    }
    if (!found) {
        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MS5525[%u]: no sensor found", sensor_as_backend_get_instance(as_backend));
        console_printf("MS5525[%u]: no sensor found", sensor_as_backend_get_instance(as_backend));
        return false;
    }

    // Send a command to read temperature first
    uint8_t reg = REG_CONVERT_TEMPERATURE;
    devmgr_transfer(ms5525->dev, &reg, 1, NULL, 0);
    ms5525->state = 0;
    ms5525->command_send_us = time_micros();

    devmgr_set_device_type(ms5525->dev, AS_DEV_TYPE_MS5525);
    sensor_as_backend_set_bus_id(as_backend, devmgr_get_bus_id(ms5525->dev));

    // drop to 2 retries for runtime
    devmgr_set_retries(ms5525->dev, 2);

    // read at 80Hz
    devmgr_register_periodic_callback(ms5525->dev, 1000000UL/80U, timer, ms5525);
    return true;
}

// return the current differential_pressure in Pascal
static bool get_differential_pressure(sensor_as_backend_t as_backend, float *_pressure)
{
    sensor_as_ms5525_t ms5525 = (sensor_as_ms5525_t)as_backend;

    rt_mutex_take(ms5525->backend._mutex, RT_WAITING_FOREVER);

    if ((time_millis() - ms5525->last_sample_time_ms) > 100) {
        rt_mutex_release(ms5525->backend._mutex);
        return false;
    }

    if (ms5525->press_count > 0) {
        ms5525->pressure = ms5525->pressure_sum / ms5525->press_count;
        ms5525->press_count = 0;
        ms5525->pressure_sum = 0;
    }
    *_pressure = ms5525->pressure;

    rt_mutex_release(ms5525->backend._mutex);

    return true;
}

// return the current temperature in degrees C, if available
static bool get_temperature(sensor_as_backend_t as_backend, float *_temperature)
{
    sensor_as_ms5525_t ms5525 = (sensor_as_ms5525_t)as_backend;

    rt_mutex_take(ms5525->backend._mutex, RT_WAITING_FOREVER);

    if ((time_millis() - ms5525->last_sample_time_ms) > 100) {
        rt_mutex_release(ms5525->backend._mutex);
        return false;
    }

    if (ms5525->temp_count > 0) {
        ms5525->temperature = ms5525->temperature_sum / ms5525->temp_count;
        ms5525->temp_count = 0;
        ms5525->temperature_sum = 0;
    }

    *_temperature = ms5525->temperature;

    rt_mutex_release(ms5525->backend._mutex);

    return true;
}

/**
 * CRC used by MS pressure devices
 */
static uint16_t crc4_prom(sensor_as_ms5525_t ms5525)
{
    return math_crc_crc4(ms5525->prom);
}

static bool read_prom(sensor_as_ms5525_t ms5525)
{
    // reset the chip to ensure it has correct prom values loaded
    uint8_t reg = REG_RESET;
    if (!devmgr_transfer(ms5525->dev, &reg, 1, NULL, 0)) {
        return false;
    }
    rt_thread_mdelay(5);

    bool all_zero = true;
    for (uint8_t i = 0; i < 8; i++) {
        be16_t val;
        if (!devmgr_read_registers(ms5525->dev, REG_PROM_BASE+i*2, (uint8_t *) &val,
                                 sizeof(uint16_t))) {
            return false;
        }
        ms5525->prom[i] = be16toh(val);
        if (ms5525->prom[i] != 0) {
            all_zero = false;
        }
    }

    if (all_zero) {
        return false;
    }

    /* save the read crc */
    const uint16_t crc_read = ms5525->prom[7] & 0xf;

    /* remove CRC byte */
    ms5525->prom[7] &= 0xff00;

    uint16_t crc_calc = crc4_prom(ms5525);
    if (crc_read != crc_calc) {
        console_printf("MS5525: CRC mismatch 0x%04x 0x%04x\n", crc_read, crc_calc);
    }
    return crc_read == crc_calc;
}

/*
  read from the ADC
 */
static int32_t read_adc(sensor_as_ms5525_t ms5525)
{
    uint8_t val[3];
    if (!devmgr_read_registers(ms5525->dev, REG_ADC_READ, val, 3)) {
        return 0;
    }
    return (val[0] << 16) | (val[1] << 8) | val[2];
}

/*
  calculate pressure and temperature
 */
static void calculate(sensor_as_ms5525_t ms5525)
{
    // table for the 001DS part, 1PSI range
    const uint8_t Q1 = 15;
    const uint8_t Q2 = 17;
    const uint8_t Q3 = 7;
    const uint8_t Q4 = 5;
    const uint8_t Q5 = 7;
    const uint8_t Q6 = 21;

    float dT = (float)(ms5525->D2) - (int64_t)(ms5525->prom[5]) * (1L<<Q5);
    float TEMP = 2000 + (dT*(int64_t)(ms5525->prom[6]))/(1L<<Q6);
    float OFF  = (int64_t)(ms5525->prom[2])*(1L<<Q2) + ((int64_t)(ms5525->prom[4])*dT)/(1L<<Q4);
    float SENS = (int64_t)(ms5525->prom[1])*(1L<<Q1) + ((int64_t)(ms5525->prom[3])*dT)/(1L<<Q3);
    float P = ((float)(ms5525->D1)*SENS/(1L<<21)-OFF)/(1L<<15);
    const float PSI_to_Pa = 6894.757f;
    float P_Pa = PSI_to_Pa * 1.0e-4 * P;
    float Temp_C = TEMP * 0.01;

#if 0
    static uint16_t counter;
    if (counter++ == 100) {
        printf("P=%.6f T=%.2f D1=%d D2=%d\n", P_Pa, Temp_C, D1, D2);
        counter=0;
    }
#endif

    rt_mutex_take(ms5525->backend._mutex, RT_WAITING_FOREVER);

    ms5525->pressure_sum += P_Pa;
    ms5525->temperature_sum += Temp_C;
    ms5525->press_count++;
    ms5525->temp_count++;
    ms5525->last_sample_time_ms = time_millis();

    rt_mutex_release(ms5525->backend._mutex);
}

// 80Hz timer
static void timer(void *parameter)
{
    sensor_as_ms5525_t ms5525 = (sensor_as_ms5525_t)parameter;

    if (time_micros() - ms5525->command_send_us < 10000) {
        // we should avoid trying to read the ADC too soon after
        // sending the command
        return;
    }
    
    uint32_t adc_val = read_adc(ms5525);

    if (adc_val == 0) {
        // we have either done a read too soon after sending the
        // request or we have tried to read the same sample twice. We
        // re-send the command now as we don't know what state the
        // sensor is in, and we do want to trigger it sending a value,
        // which we will discard
        if (devmgr_transfer(ms5525->dev, &ms5525->cmd_sent, 1, NULL, 0)) {
            ms5525->command_send_us = time_micros();
        }
        // when we get adc_val == 0 then then both the current value and
        // the next value we read from the sensor are invalid
        ms5525->ignore_next = true;
        return;
    }
    
    /*
     * If read fails, re-initiate a read command for current state or we are
     * stuck
     */
    if (!ms5525->ignore_next) {
        if (ms5525->cmd_sent == REG_CONVERT_TEMPERATURE) {
            ms5525->D2 = adc_val;
        } else if (ms5525->cmd_sent == REG_CONVERT_PRESSURE) {
            ms5525->D1 = adc_val;
            calculate(ms5525);
        }
    }

    ms5525->ignore_next = false;

    ms5525->cmd_sent = (ms5525->state == 0) ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
    if (!devmgr_transfer(ms5525->dev, &ms5525->cmd_sent, 1, NULL, 0)) {
        // we don't know for sure what state the sensor is in when we
        // fail to send the command, so ignore the next response
        ms5525->ignore_next = true;
        return;
    }
    ms5525->command_send_us = time_micros();

    ms5525->state = (ms5525->state + 1) % 5;
}

/*------------------------------------test------------------------------------*/


